#ifndef QNODE_NEW_H
#define QNODE_NEW_H

#ifndef Q_MOC_RUN
#include <ros/ros.h>
#endif

#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <move_base_action/CommonMovebaseAction.h>
#include <move_base_action/CommonMovebaseGoal.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <move_base_msgs/MoveBaseGoal.h>
#include <ros/network.h>
#include <ros/ros.h>
#include <ros_qt_msgs/AdjustSpeed.h>
#include <ros_qt_msgs/Msg.h>
#include <ros_qt_msgs/NavStatus.h>
#include <ros_qt_msgs/QnodeCommonServiceinterface.h>
#include <ros_qt_msgs/RefReset.h>
#include <ros_qt_msgs/Relocation.h>
#include <ros_qt_msgs/SetMoveMode.h>
#include <ros_qt_msgs/TargetPose.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_srvs/SetBool.h>
#include <tf/transform_datatypes.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <QDebug>
#include <QThread>
#include <QTimer>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <station.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int8.h>
#include <std_msgs/String.h>

#define coordinateConversionCoefficient 1000.0
#define speedConversionCoefficient 100.0

namespace qnode_new {

typedef actionlib::SimpleActionClient< move_base_msgs::MoveBaseAction >         MoveBaseClient;
typedef actionlib::SimpleActionClient< move_base_action::CommonMovebaseAction > CommonMovebaseClient;

class Publishers {
public:
    ros::NodeHandle    nh{ "ros_qt" };
    ros::Publisher     coommonPublishInterfacePub{ nh.advertise< std_msgs::String >( "/qnode/common/publish_interface", 1 ) };
    ros::ServiceClient commonServiceInterfaceClient{ nh.serviceClient< ros_qt_msgs::QnodeCommonServiceinterface >( "/qnode/common/service_interface" ) };
};

class QNodeNew : public QThread {
    Q_OBJECT

Q_SIGNALS:
    void sigSendTips( const QString& );
    void sigRosShutdown();  //正常运行时候发生严重错误，再调用发生给qt

private:
    int                                       init_argc;
    char**                                    init_argv;
    bool                                      is_rosmaster_run = false;
    boost::shared_ptr< Publishers >           publishers;
    boost::shared_ptr< CommonMovebaseClient > commonClient;
    void                                      run() {
        int code = main();
        if ( code < 0 ) { Q_EMIT sigRosShutdown(); }
    }

public:
    QNodeNew( int argc, char** argv ) : init_argc( argc ), init_argv( argv ) {}

    ~QNodeNew() {
        if ( ros::isStarted() ) {
            Q_EMIT sigRosShutdown();
            ros::shutdown();
            ros::waitForShutdown();
        }
        wait();
    }

    int main() {
        ros::init( init_argc, init_argv, "qnode_qt" );
        if ( !ros::master::check() ) {
            Q_EMIT sigSendTips( QString( "错误：ros master 可能未运行" ) );
            return -1;
        }
        ros::start();
        ros::NodeHandle nh( "ros_qt" );
        publishers = boost::make_shared< Publishers >();

        Q_EMIT          sigSendTips( QString( "提示：ros qnode 初始化完成" ) );
        ros::Subscriber commonSub = nh.subscribe< std_msgs::String >( "/qnode/common/subscription_interface", 1, &QNodeNew::onCommonSubcription_BC, this );
        commonClient              = boost::make_shared< CommonMovebaseClient >( "/common/move_base_action", true );
        is_rosmaster_run          = true;
        ros::spin();
        return 0;
    }
    void onCommonSubcription_BC( const std_msgs::String sub_bc_json ) {
        qDebug() << "订阅，返回的数据:" << sub_bc_json.data.c_str();
    }
public Q_SLOTS:
    QByteArray slotCommonService( QString json ) {
        ros_qt_msgs::QnodeCommonServiceinterface data;
        data.request.data = json.toStdString();
        publishers->commonServiceInterfaceClient.call( data );
        return data.response.data.c_str();
    }
    void slotCommonPblish( QString json ) {
        std_msgs::String data;
        data.data = json.toStdString();
        publishers->coommonPublishInterfacePub.publish( data );
    }
    void slotCommonMoveBaseAction( int mode, QString json ) {
        move_base_action::CommonMovebaseGoal goal;
        goal.mode = mode;
        goal.data = json.toStdString();
        if ( !commonClient->waitForServer( ros::Duration( 3 ) ) ) {
            qDebug() << "错误：无法连接SLAM导航服务！";
            return;
        }
        commonClient->sendGoal( goal, boost::bind( &QNodeNew::common_move_base_CB, this, _1, _2 ) );
    }

    void common_move_base_CB( const actionlib::SimpleClientGoalState& state, const boost::shared_ptr< const move_base_action::CommonMovebaseResult >& ) {
        if ( state.isDone() ) {
            qDebug() << "common_move_base_CB:" << state.state_;
            switch ( state.state_ ) {
            case actionlib::SimpleClientGoalState::SUCCEEDED:
                break;
            case actionlib::SimpleClientGoalState::PREEMPTED:
                break;
            case actionlib::SimpleClientGoalState::PENDING:
            case actionlib::SimpleClientGoalState::ACTIVE:
            case actionlib::SimpleClientGoalState::RECALLED:
            case actionlib::SimpleClientGoalState::REJECTED:
            case actionlib::SimpleClientGoalState::ABORTED:
            case actionlib::SimpleClientGoalState::LOST:
                break;
            }
        }
    }
};

}  // namespace qnode

#endif  // QNODE_NEW_H
